#ifndef CONTROL_HPP
#define CONTROL_HPP

#include <cstdint>

// 系统常量
constexpr int NUM_MOTORS = 8;
constexpr int NUM_CONTROLS = 3;
constexpr float MAX_THRUST = 3.0f;  // 最大推力限制

// 预计算得到的伪逆矩阵 (8x3)，通过MATLAB pinv(B)计算
// 电机顺序: [对角1, 对角2, 对角3, 对角4, X+, X-, Y+, Y-]
// 控制量顺序: [Fx, Fy, Mz]
extern const float B_pinv[8][3];

// 控制指令结构体
struct ControlCommand {
    float Fx;  // X方向力 (N)
    float Fy;  // Y方向力 (N)
    float Mz;  // 偏航力矩 (Nm)
};

// 八推力分配函数
void eight_thrust_allocate(const ControlCommand* v_d, float u[8]);

/*
电机型号：A2212
KV值：KV980
螺旋桨：8045
8.0英寸桨叶直径：8*2.54=20.32cm
4.5英寸的螺距，意味着桨叶旋转一周，飞机前进的距离
桨直径越大升力越大
螺距越大速度越快
一般选大直径小螺距，适合低空悬停
小直径大螺距，适合高转速，如穿越机
*/

#endif // THRUST_ALLOCATOR_HPP
